/**
* @file        NaviUtilTools
* @brief       
* @version     1.0.0
* @author      Changlin.Jing 
* @date        2023/8/23 16:21
* @copyright   Copyright  2022 Langge Software Co., Ltd. All rights reserved.
*/
#ifndef LANGGE_VDR_NAVIATTITUDETOOLS_HPP
#define LANGGE_VDR_NAVIATTITUDETOOLS_HPP
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace UtilTools::Attitude{
    using RotationVector = Eigen::Vector3d;
    using Euler = Eigen::Vector3d; /*roll, pitch, heading*/

    /**
     * @brief  quaternion to rotation martrix by Eigen
     * @note
     * @param  q: quaternion
     * @retval rotation matrix
     */
    Eigen::Matrix3d Quaternion2RotationMatrix(const Eigen::Quaterniond &q);

    /**
      * @brief  rotation martrix to quaternion by Eigen
      * @note
      * @param  mat:
      * @retval
      */
    Eigen::Quaterniond RotationMartix2Quaternion(const Eigen::Matrix3d &mat);

    /**
     * @brief
     * @note
     * @param  &euler:
     * @retval
     */
    Eigen::Quaterniond Euler2Quaternion(const Euler &euler);

    /**
     * @brief  Euler angle to rotation matrix
     * @note
     * @param  euler: Euler angle
     * @retval
     */
    Eigen::Matrix3d Euler2RotationMatrix(const Euler &euler);

    /**
      * @brief  Rotation Matrix to Euler by Eigen
      * @note   note the order of the axis
      * @param  &mat:
      * @retval
      */
    Euler RotationMartix2Euler(const Eigen::Matrix3d &mat);

    /**
     * @brief  quaternion to euler by rotation matirx
     * @note
     * @param  q: quaternion
     * @retval
     */
    Euler Quaternion2Euler(const Eigen::Quaterniond &q);

    /**
     * @brief  according the rotation vector construct the Quaternion
     * @note
     * @param  rv: Equivalent rotation vector
     * @retval
     */
    Eigen::Quaterniond RotationVector2Quaternion(const RotationVector &rv);

    /***
     * 四元数转等效旋转矢量
     * @param quaternion
     * @return
     */
    Eigen::Vector3d Quaternion2Vector(const Eigen::Quaterniond &quaternion);

    /**
     * @brief 将等效旋转矢量转为四元数
     * @param rotvec
     * @return
     */
    Eigen::Quaterniond RotVec2Quaternion(const Eigen::Vector3d &rotvec);

    /**
     * @brief  Rotation Vector to Rotation Matrix
     * @note
     * @param  &rv: Equivalent rotation vector
     * @retval
     */
    //Eigen::Matrix3d RotationVector2RotationMatrix(const RotationVector &rv);

    /**
     * @brief  skew matrix from vector
     * @note
     * @param  vector:
     * @retval
     */
    Eigen::Matrix3d Skew(const Eigen::Vector3d &vector);

    float WrapRadian_2Pi(float radian);

    float AbsSubAngle(float angle1, float angle2);

    template <typename Derived>
    inline Eigen::Matrix<Derived, 3, 1> eulerAngles(const Eigen::Matrix<Derived, 3, 3> &coeff){
        Eigen::Matrix<Derived, 3, 1> res;

        const size_t i = 2;
        const size_t j = 1;
        const size_t k = 0;
        typedef Eigen::Matrix<Derived, 2, 1> Vector2;

        res[2] = atan2(coeff(j, k), coeff(k, k));
        Derived c2 = Vector2(coeff(i, i), coeff(i, j)).norm();
        res[1] = atan2(-coeff(i, k), c2);
        Derived s1 = sin(res[2]);
        Derived c1 = cos(res[2]);
        res[0] = atan2(s1 * coeff(k, i) - c1 * coeff(j, i), c1 * coeff(j, j) - s1 * coeff(k, j));

        return res;
    }

}


#endif //LANGGE_VDR_NAVIATTITUDETOOLS_HPP
